/*************************************** Copyright ******************************
  * FileName   : AS5600.c
  * Version    : v1.0
  * Author     : --
  * Date       : 2022-10-22
  * Description:
  * Function List:
  	1. ....
  	   <version>:
  <modify staff>:
  		  <data>:
   <description>:
  	2. ...
********************************************************************************/

/*include----------------------------------------------------------------------*/
#include "AS5600.h"
#include "tim.h"
/*define-----------------------------------------------------------------------*/


/*variate----------------------------------------------------------------------*/


/*statement--------------------------------------------------------------------*/


/*Function prototype Begin*****************************************************/

#include "as5600.h"

#define abs(x) ((x)>0?(x):-(x))
#define _2PI 6.28318530718

HAL_StatusTypeDef AS5600_Check(void);
static float angle_data_prev; //上次位置
static float full_rotation_offset; //转过的整圈数

void bsp_as5600Init(void)
{
    /* init i2c interface */

    /* init var */
    full_rotation_offset = 0;
    angle_data_prev = bsp_as5600GetRawAngle();
}

/***********************************************************
*@fuction	:bsp_as5600GetRawAngle
*@brief		:返回磁编码器值
*@param		:--
*@return	:磁编码器值
*@author	:--
*@date		:2022-10-22
***********************************************************/

uint16_t bsp_as5600GetRawAngle(void)
{
    uint16_t raw_angle;
    uint8_t buffer[2] = {0};
    uint8_t raw_angle_register = AS5600_RAW_ANGLE_REGISTER;		//芯片角度信息寄存器
		while(hi2c2.State != HAL_I2C_STATE_READY);
		
		HAL_StatusTypeDef status;		
		status = HAL_I2C_Mem_Read(&hi2c2,AS5600_ADDR,raw_angle_register,1,buffer,2,1);		//75us
//		status = HAL_I2C_Mem_Read_DMA(&hi2c2,AS5600_ADDR,raw_angle_register,1,buffer,2);		
		if(status != HAL_OK) 
		{
			HAL_I2C_DeInit(&hi2c2);
			MX_I2C2_Init();
			status = HAL_I2C_Mem_Read(&hi2c2,AS5600_ADDR,raw_angle_register,1,buffer,2,1);		//75us
			printf_uart2("IIC2 ERR:%d\n",status);
		}
    raw_angle = ((uint16_t)buffer[0] << 8) | (uint16_t)buffer[1];
    return AS5600_RESOLUTION-raw_angle;
}

/***********************************************************
*@fuction	:bsp_as5600GetAngle
*@brief		:返回AS5600检测的转过的绝对角度，与上一次检测旋转角度不能大于0.8圈，否则判断为反转。
*@param		:--
*@return	:float
*@author	:--
*@date		:2022-10-22
***********************************************************/
float bsp_as5600GetAngle(void)
{
    float angle_data = bsp_as5600GetRawAngle();

    float d_angle = angle_data - angle_data_prev;
    if(abs(d_angle) > (0.8 * AS5600_RESOLUTION))
    {
        full_rotation_offset += (d_angle > 0 ? -_2PI : _2PI);
    }
    angle_data_prev = angle_data;

    return (full_rotation_offset + ((double)angle_data / (double)AS5600_RESOLUTION) * _2PI);
}

HAL_StatusTypeDef AS5600_Check()
{
		return	HAL_I2C_IsDeviceReady (&hi2c2,AS5600_ADDR,1,5);
}

/*Function prototype End*******************************************************/


